package com.bignerdranch.android.sensorapplication;

import Jama.Matrix;

public class CoordinateSystemRotation {

    public static Matrix coorinateSystemRotate(float axis[], float omega, float vector[]) {
        double myRotationM[][] = getRotationMatrix(axis, omega);
        Matrix mR = new Matrix(myRotationM);
        Matrix inverMR = mR.inverse();

        Matrix gravM = new Matrix(vector.length,1);
        for(int i = 0; i < vector.length; i++) {
            gravM.set(i, 0, vector[i]);
        }
        return inverMR.times(gravM);
    }

    /**
     * 获取旋转矩阵
     * @param v1
     * @param angle
     * @return
     */
    public static double[][] getRotationMatrix(float v1[],float angle) {
        float cost = (float) Math.cos(angle);
        float v[] = normalise(v1);
        float sint = (float) Math.sin(angle);
        float one_sub_cost = 1 - cost;
        return new double[][]{{v[0]*v[0]*one_sub_cost+cost,v[0]*v[1]*one_sub_cost-v[2]*sint,v[0]*v[2]*one_sub_cost+v[1]*sint},
                {v[0]*v[1]*one_sub_cost+v[2]*sint,v[1]*v[1]*one_sub_cost+cost,v[1]*v[2]*one_sub_cost-v[0]*sint},
                {v[0]*v[2]*one_sub_cost-v[1]*sint,v[1]*v[2]*one_sub_cost+v[0]*sint,v[2]*v[2]*one_sub_cost+cost}};
    }

    private static float[] normalise(float v1[]) {
        float v[] = new float[3];
        float sum = 0;
        for(int i = 0; i < v.length; i++) {
            v[i] = v1[i];
            sum += v[i]*v[i];
        }
        double module = Math.sqrt(sum);
        for(int i = 0; i < v.length; i++) {
            v[i] /= module;
        }
        return v;
    }
}
